/*************************************************************************
    Projet  -  Sport Collectif pour Robots
                             -------------------
    Auteur  -  Kevin Vandel
*************************************************************************/

//-- Interface de la classe <ControleRobot> (fichier ControleRobot.h) ----
#ifndef CONTROLE_ROBOT_H_
#define CONTROLE_ROBOT_H_


//------------------------------------------------------------------------
// Role de la classe <Robot>
// 	Gere les actions d'un robot.
//	Comportement differencie selon phase attaque ou defense.
//------------------------------------------------------------------------


//--------------------------------------------------- Interfaces utilisees
#include "Communication.h"
#include "../config.h"
#include "../util.h"
#include "../MsgQueue.h"
#include "../Vision/VideoSource.h"
#include "../IA/Strategie.h"

#ifdef __APPLE__	// mac
#include <opencv/openCV.h>
#endif


#ifdef __linux__	// gcc (egcs) on linux
#include <opencv.hpp>
#endif

using namespace cv;

//------------------------------------------------------------------ Types
enum positionPostRotation { OPTIMALE, AVANT, ARRIERE };


class ControleRobot
{
//----------------------------------------------------------------- PUBLIC
public:
//----------------------------------------------------- Methodes publiques
	bool initCom();
	// Mode d'emploi :
	//	Initialise l'interface de communication du robot.
	//	Retourne vrai si l'operation reussit, false sinon.

	void tirer(bool rotationGauche);
	// Mode d'emploi :
	//	Commande au robot de tirer en tournant sur lui-meme.
	//	Le robot tournera sur sa gauche si "rotationGauche" est vrai
	//	et sur sa droite sinon.

	bool tourner(Vec2d direction, positionPostRotation posRot = OPTIMALE);
	// Mode d'emploi :
	//	Tourne le robot sur lui-meme jusqu'a ce que sa direction coincide
	//	avec l'axe du vecteur "direction".
	//	On peut egalement specifier le sens dans lequel on souhaite que le
	//	robot se trouve apres sa rotation (par defaut le robot ne se soucie
	//	pas de ce sens et tourne de la maniere la plus rapide).
	//	Renvoie true si la rotation est arrivee a terme et false sinon.

	bool avancer(double distance, bool marcheAvant = true);
	// Mode d'emploi :
	//	Demarre le robot tout droit sur une certaine "distance" (en centimetre),
	//	en marche avant ou marche arriere selon la valeur de "marcheAvant".
	//	Renvoie true si la rotation est arrivee a terme et false sinon.

	bool arreterRobot();
	// Mode d'emploi :
	//	Arrete le robot (vitesse nulle).
	//	Renvoie true si la commande fonctionne et false sinon.

	bool LancerControle();
	// Mode d'emploi :
	//	Lance le processus de controle du robot.

	int ArreterControle();
	// Mode d'emploi :
	//	Met fin au processus de controle du robot.

//-------------------------------------------- Constructeurs - destructeur
	ControleRobot(string port, int numeroRob, MsgQueue<DataVision>* _visionQueue, MsgQueue<DataIA>* _iAQueue);

	~ControleRobot();


//---------------------------------------------------------------- PROTEGE
protected:
//------------------------------------------------------- Attributs prives
	string usbPort;				//Port de communication
	int idRob;					//Identifiant du robot
	Communication comKhepera;	//Interface de communication du robot

	bool marcheAvant;			//Indique si le robot doit se d�placer en marche avant
	bool controleThreadLoop;
	bool exit;

	MsgQueue<DataVision>* visionQueue;
	MsgQueue<DataIA>* iAQueue;
	DataVision visionData;
	DataIA iaData;

	pthread_mutex_t lockVisionData;
	pthread_mutex_t lockIaData;
	pthread_mutex_t lockControleThreadLoop;
	pthread_mutex_t lockExit;
	pthread_mutex_t lockAsservissement;
	pthread_mutex_t lockArrive;

	pthread_cond_t asservissement;
	pthread_cond_t arrive;

	pthread_t visionThreadId;
	pthread_t asservirThreadId;
	pthread_t controlerThreadId;

	struct val_st<ControleRobot>* visionParams;
	struct val_st<ControleRobot>* asservirParams;
	struct val_st<ControleRobot>* controlerParams;

//----------------------------------------------------- Methodes protegees

	bool resetPositionRoues();
	// Mode d'emploi :
	//	Met a zero le compteur de position des roues du robot.
	//	Renvoie true si la commande fonctionne et false sinon.


	void visionThread();
	void asservirThread();
	void controlerThread();
	void ( ControleRobot::*visionThreadWrapper() ) ()
	{
		return &ControleRobot::visionThread;
	}
	void ( ControleRobot::*asservirThreadWrapper() ) ()
	{
		return &ControleRobot::asservirThread;
	}
	void ( ControleRobot::*controlerThreadWrapper() ) ()
	{
		return &ControleRobot::controlerThread;
	}

	//bool asservirRobot();
	// Mode d'emploi :
	//	Asservit le deplacement du robot et controle l'arrivee a destination.
	//	Retourne true si le robot est arrive a destination et false sinon.

	bool isControleThreadRunning();
	bool isExit();
	void setExit(bool b);
	void getPosition(Position& posRob);
	void getPositionBalle(Position& posBalle);
	void setVisionData(DataVision data);
	void getObjPosition(Position& posObjRob);
	void setIaData(DataIA data);

//------------------------------------------------------------------ PRIVE
private:
//------------------------------------------------------- Methodes privees
	ControleRobot(); //Interdit la creation par defaut
};

#endif // CONTROLE_ROBOT_H_
